/**
  ******************************************************************************
  * @file           : chassis_motor_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/8
  ******************************************************************************
  */



#include "chassis_motor_task.h"

extern rc_device_t g_rc;
extern INS_t INS;
extern float yaw_total_rounds;
extern float yaw_gyro;
extern radar_speed speed_radar;
extern ChassisPowerDataStruct Power;
extern float chassis_gyro;
extern float yaw_current_rounds;
extern uint8_t is_flip_over;
extern uint8_t is_on_slope;
extern chassis_yaw_info chassis_yaw_cal;
extern uint8_t is_use_capacitor;
extern receive_judge_t judge_rece_mesg;

float left_front_rudder_rounds = 0;
float left_back_rudder_rounds = 0;
float right_back_rudder_rounds = 0;
float right_front_rudder_rounds = 0;

float left_front_wheel_speed = 0;
float left_back_wheel_speed = 0;
float right_back_wheel_speed = 0;
float right_front_wheel_speed = 0;

extern float current_speed_array[8];
extern float vel_pid_out_array[8];

float optimal_rounds = 0;
float delta_theta = 0;
float vx = 0;
float vy = 0;
float spin_omega = 1.0;
float speed_coefficient = 1.0f;

float rounds2rad(float rounds){ //返回值为0-2pi
    float rad;
    rad = (rounds - floor(rounds)) * 2 * PI;
    return(rad);
}

float rounds2currentRounds(float rounds){  //返回值为0-1
    float currentRounds;
    currentRounds = rounds - floor(rounds);
    return(currentRounds);
}

float rad2rounds(float rad){
    return(rad / 2.f / PI);
}

float compare_closest(float A,float B,float current){
    if(abs(A - current) < abs(B - current)){
        return(A);
    }else{
        return(B);
    }
}



void motor_optimal_solute(float total_rounds, float rad_cal, float speed_cal, float &rounds_target, float &speed_target){
    //统一到0~2pi
    float rounds_rad = rounds2rad(total_rounds);
    rad_cal = rad_cal < 0 ? rad_cal + 2 * PI : rad_cal;

    if((abs(rounds_rad - rad_cal) < PI / 2.f + OPTIMAL_REDUNDANCY) || (abs(rounds_rad - rad_cal) > PI * 1.5f - OPTIMAL_REDUNDANCY)){
        float round_target1 = floor(total_rounds) + rad2rounds(rad_cal);
        float round_target2 = floor(total_rounds) + rad2rounds(rad_cal) - 1;
        float round_target3 = floor(total_rounds) + rad2rounds(rad_cal) + 1;
        rounds_target = compare_closest(compare_closest(round_target1, round_target2, total_rounds), round_target3, total_rounds);
        speed_target = speed_cal;
    }else{
        float round_target1 = floor(total_rounds) + rad2rounds(rad_cal) - 0.5;
        float round_target2 = floor(total_rounds) + rad2rounds(rad_cal) - 0.5 - 1;
        float round_target3 = floor(total_rounds) + rad2rounds(rad_cal) - 0.5 + 1;
        rounds_target = compare_closest(compare_closest(round_target1, round_target2, total_rounds), round_target3, total_rounds);
        speed_target = -speed_cal;
    }
}


void rounds_optimal_solute(float total_rounds, float target_current_rounds, float &target_total_rounds){
    float target_cal = rounds2currentRounds(target_current_rounds);

    float round_target1 = floor(total_rounds) + target_cal;
    float round_target2 = floor(total_rounds) + target_cal - 1;
    float round_target3 = floor(total_rounds) + target_cal + 1;
    target_total_rounds = compare_closest(compare_closest(round_target1, round_target2, total_rounds), round_target3, total_rounds);
}

extern float k_power_rudder;
motion_type motionType = normal;
uint8_t mains_power_chassis_output_last = 0;
uint8_t mains_power_chassis_output = 0;
int8_t rotate_sense = -1;
void chassis_motor_task(void *argument){
    power_param_init();
    RC_SW sw_l_last = RC_SW_NONE;
    static pid omega_ctrl(1.0f, 0.2, 1.f, 0.000, 1.f);  //底盘跟随，输出omega
    omega_ctrl.pid_set_dead_band(0.01);

    static LK_motor left_front_rudder(&hcan1, nullptr, 0, 3, ms_6015, 1, 0);
    static LK_motor left_back_rudder(&hcan1, nullptr, 0, 2, ms_6015, 1, 0);
    static LK_motor right_back_rudder(&hcan1, nullptr, 0, 1, ms_6015, 1, 0);
    static LK_motor right_front_rudder(&hcan1, nullptr, 0, 4, ms_6015, 1, 0);

    left_front_rudder.velPid.pid_reset(1.0f, 0.1f, 4.f, 0.00f, 0.1f);
    left_front_rudder.posPid.pid_reset(1.0f, 0.1f, 4.f, 0.f, 0.f);
    left_back_rudder.velPid.pid_reset(1.0f, 0.1f, 4.f, 0.00f, 0.1f);
    left_back_rudder.posPid.pid_reset(1.0f, 0.1f, 4.f, 0.f, 0.f);
    right_back_rudder.velPid.pid_reset(1.0f, 0.1f, 4.f, 0.00f, 0.1f);
    right_back_rudder.posPid.pid_reset(1.0f, 0.1f, 4.f, 0.f, 0.f);
    right_front_rudder.velPid.pid_reset(1.0f, 0.1f, 4.f, 0.00f, 0.1f);
    right_front_rudder.posPid.pid_reset(1.0f, 0.1f, 4.f, 0.f, 0.f);

    left_front_rudder.motor_set_offset(LEFT_FRONT_OFFSET);
    left_back_rudder.motor_set_offset(LEFT_BACK_OFFSET);
    right_back_rudder.motor_set_offset(RIGHT_BACK_OFFSET);
    right_front_rudder.motor_set_offset(RIGHT_FRONT_OFFSET);

    left_front_rudder.power_limit = &k_power_rudder;
    left_back_rudder.power_limit = &k_power_rudder;
    right_back_rudder.power_limit = &k_power_rudder;
    right_front_rudder.power_limit = &k_power_rudder;

    static DJI_motor left_back_wheel(&hcan2, nullptr, 2, DJI_M3508, 1);
    static DJI_motor right_back_wheel(&hcan2, nullptr, 1, DJI_M3508, 1);
    static DJI_motor right_front_wheel(&hcan2, nullptr, 4, DJI_M3508, 0);
    static DJI_motor left_front_wheel(&hcan2, nullptr, 3, DJI_M3508, 0);


    left_front_wheel.velPid.pid_reset(1.0f, 0.1f, 12.f, 0.00f, 0.1f);
    left_back_wheel.velPid.pid_reset(1.0f, 0.1f, 12.f, 0.00f, 0.1f);
    right_back_wheel.velPid.pid_reset(1.0f, 0.1f, 12.f, 0.00f, 0.1f);
    right_front_wheel.velPid.pid_reset(1.0f, 0.1f, 12.f, 0.00f, 0.1f);

    left_front_wheel.power_limit = &Power.k;
    left_back_wheel.power_limit = &Power.k;
    right_back_wheel.power_limit = &Power.k;
    right_front_wheel.power_limit = &Power.k;
    static rudder_wheel chassis_rudder_wheel(SPEED_MAX, OMEGA_MAX, 190, 190);

    for(;;){
        mains_power_chassis_output = judge_rece_mesg.robot_status_data.mains_power_chassis_output;
        current_speed_array[0] = abs(left_front_wheel.motor_get_speed() / MOTOR_3508_MAX_SPEED);
        current_speed_array[1] = abs(left_back_wheel.motor_get_speed() / MOTOR_3508_MAX_SPEED);
        current_speed_array[2] = abs(right_back_wheel.motor_get_speed() / MOTOR_3508_MAX_SPEED);
        current_speed_array[3] = abs(right_front_wheel.motor_get_speed() / MOTOR_3508_MAX_SPEED);

        vel_pid_out_array[0] = abs(left_front_wheel.velPid.out);
        vel_pid_out_array[1] = abs(left_back_wheel.velPid.out);
        vel_pid_out_array[2] = abs(right_back_wheel.velPid.out);
        vel_pid_out_array[3] = abs(right_front_wheel.velPid.out);

        if(g_rc.is_lost == 0 && g_rc.rc_info.sw.sw_r == RC_SW_UP && is_flip_over == 0) {
            motionType = g_rc.rc_info.sw.sw_l == RC_SW_MID ? auto_navigate : gimbal_follow;
            motionType = g_rc.rc_info.sw.sw_l == RC_SW_DOWN ? gyroscope : motionType;
            rotate_sense = rotate_sense * (g_rc.rc_info.sw.sw_l == RC_SW_DOWN && sw_l_last == RC_SW_MID ? -1 : 1);

            /**----------------旋转速度设置------------------*/
            if(chassis_yaw_cal.is_spin == 0){
                low_pass(spin_omega, OMEGA_ZERO, 0.005);
            }else if(chassis_yaw_cal.is_spin == 1 && chassis_yaw_cal.high_speed_spin == 1){
                low_pass(spin_omega, OMEGA_MAX, 0.005);
            }else if(chassis_yaw_cal.is_spin == 1 && chassis_yaw_cal.high_speed_spin == 0){
                low_pass(spin_omega, OMEGA_NORMAL, 0.005);
            }

            /**----------------平移速度设置------------------*/
            if(is_on_slope == 1 || chassis_yaw_cal.slope_speed_translation == 1){
                low_pass(speed_coefficient, SPEED_SLOPE, 0.005);
            }else if(is_on_slope == 0 && chassis_yaw_cal.high_speed_translation == 1){
                low_pass(speed_coefficient, SPEED_MAX, 0.005);
            }else if(is_on_slope == 0 && chassis_yaw_cal.high_speed_translation == 0){
                low_pass(speed_coefficient, SPEED_NORMAL, 0.005);
            }

            switch (motionType) {
                case normal:  //调试用，无跟随
                    chassis_rudder_wheel.get_motion_parameter(g_rc.rc_info.rocker.left_x, g_rc.rc_info.rocker.left_y, -g_rc.rc_info.rocker.right_x);

                    break;

                case gimbal_follow:
                    rounds_optimal_solute(yaw_current_rounds, YAW_FORWARD_ROUNDS, optimal_rounds);
                    omega_ctrl.pid_calculate(optimal_rounds, yaw_current_rounds);
                    chassis_rudder_wheel.get_motion_parameter(g_rc.rc_info.rocker.left_x, g_rc.rc_info.rocker.left_y, -omega_ctrl.out);

                    break;

                    /**
                     * 这里的delta_theta需要补偿，原因在于小陀螺时过高转速会导致计算最优解的时间影响到最终作用时间
                     * 从而形成一个稳定的偏置
                     * 这里可以做一个对于omega线性相关的偏置
                     * 大yaw也一样
                     */
                case gyroscope:
                    delta_theta = -(yaw_current_rounds - YAW_FORWARD_ROUNDS - 0.05 / 0.7 * 0.7) * 2.f * PI * rotate_sense;
                    vx = arm_sin_f32(delta_theta) * g_rc.rc_info.rocker.left_y + arm_cos_f32(delta_theta) * g_rc.rc_info.rocker.left_x;
                    vy = arm_cos_f32(delta_theta) * g_rc.rc_info.rocker.left_y - arm_sin_f32(delta_theta) * g_rc.rc_info.rocker.left_x;
                    chassis_rudder_wheel.get_motion_parameter(vx * speed_coefficient, vy * speed_coefficient, 0.7 * rotate_sense);//这里的vx vy坐标系是右x，前y
//                    chassis_rudder_wheel.get_motion_parameter(vx * speed_coefficient, vy * speed_coefficient, 0);//这里的vx vy坐标系是右x，前y
                    break;

                case auto_navigate:
                    delta_theta = -(yaw_current_rounds - YAW_FORWARD_ROUNDS - 0.05 / 0.7 * spin_omega) * 2.f * PI;
                    vx = arm_sin_f32(delta_theta) * speed_radar.speed_x - arm_cos_f32(delta_theta) * speed_radar.speed_y;
                    vy = arm_cos_f32(delta_theta) * speed_radar.speed_x + arm_sin_f32(delta_theta) * speed_radar.speed_y;
                    chassis_rudder_wheel.get_motion_parameter(vx * speed_coefficient, vy * speed_coefficient, spin_omega);
//                    chassis_rudder_wheel.get_motion_parameter(vx * speed_coefficient, vy * speed_coefficient, 0);
                    break;

            }

            chassis_rudder_wheel.rudder_wheel_cal();
            motor_optimal_solute(left_front_rudder.motor_get_rounds(), chassis_rudder_wheel.rudderRound.left_front_rudder, chassis_rudder_wheel.wheelSpeed.left_front_wheel,
                                 left_front_rudder_rounds, left_front_wheel_speed);
            motor_optimal_solute(left_back_rudder.motor_get_rounds(), chassis_rudder_wheel.rudderRound.left_back_rudder, chassis_rudder_wheel.wheelSpeed.left_back_wheel,
                                 left_back_rudder_rounds, left_back_wheel_speed);
            motor_optimal_solute(right_back_rudder.motor_get_rounds(), chassis_rudder_wheel.rudderRound.right_back_rudder, chassis_rudder_wheel.wheelSpeed.right_back_wheel,
                                 right_back_rudder_rounds, right_back_wheel_speed);
            motor_optimal_solute(right_front_rudder.motor_get_rounds(), chassis_rudder_wheel.rudderRound.right_front_rudder, chassis_rudder_wheel.wheelSpeed.right_front_wheel,
                                 right_front_rudder_rounds, right_front_wheel_speed);

            if(mains_power_chassis_output == 1 && mains_power_chassis_output_last == 0){
                left_front_wheel.motor_set_current(0);
                left_back_wheel.motor_set_current(0);
                right_back_wheel.motor_set_current(0);
                right_front_wheel.motor_set_current(0);
                left_front_rudder.motor_set_current(0);
                left_back_rudder.motor_set_current(0);
                right_back_rudder.motor_set_current(0);
                right_front_rudder.motor_set_current(0);
                osDelay(6000);
            }
            left_front_rudder.motor_set_rounds(left_front_rudder_rounds);
            left_back_rudder.motor_set_rounds(left_back_rudder_rounds);
            right_back_rudder.motor_set_rounds(right_back_rudder_rounds);
            right_front_rudder.motor_set_rounds(right_front_rudder_rounds);

            left_front_wheel.motor_set_speed(left_front_wheel_speed);
            left_back_wheel.motor_set_speed(left_back_wheel_speed);
            right_back_wheel.motor_set_speed(right_back_wheel_speed);
            right_front_wheel.motor_set_speed(right_front_wheel_speed);

//            left_front_wheel.motor_set_speed(0);
//            left_back_wheel.motor_set_speed(0);
//            right_back_wheel.motor_set_speed(0);
//            right_front_wheel.motor_set_speed(0);

        }else{
            left_front_rudder.motor_set_current(0);
            left_back_rudder.motor_set_current(0);
            right_back_rudder.motor_set_current(0);
            right_front_rudder.motor_set_current(0);

            left_front_wheel.motor_set_current(0);
            left_back_wheel.motor_set_current(0);
            right_back_wheel.motor_set_current(0);
            right_front_wheel.motor_set_current(0);
        }

        mains_power_chassis_output_last = mains_power_chassis_output;
        sw_l_last = g_rc.rc_info.sw.sw_l;
        osDelay(1);
    }

}